/**
  ******************************************************************************
  * @file    main.cc
  * @author  xinkai yu
  * @version V1.1.0
  * @date    2022/02/25
  * @brief   arm kinematic node for MobiRo gen3 @ TIB330
  ******************************************************************************
  * @attention
  *
  ******************************************************************************
  */

#include <iostream>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>  // display bug
#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <std_msgs/UInt8.h>
#include "std_srvs/Empty.h"
#include "arm_base/joint_angular.h"
#include "std_msgs/Float64MultiArray.h" 
#include "eigen_conversions/eigen_msg.h"



using namespace Eigen;

class ArmKinematic {
 public:
  explicit ArmKinematic() {
    //订阅了目标物体位置，有个flag没搞懂
    target_pose_sub_= nh_.subscribe(
        "target_pose", 1, &ArmKinematic::InverseKine, this);

    //发布关节状态，通过这个来让机械臂动,这个不需要了
    arm_conf_pub_ = nh_.advertise<arm_base::joint_angular>("target_conf", 1);

    nav_conf_pub_ = nh_.advertise<arm_base::joint_angular>("nav_target_conf", 1);

    //订阅搜索状态，并进行后处理，0是啥时候发布的
    search_cmd_sub_ = nh_.subscribe(
        "target_search", 1, &ArmKinematic::SearchCallback, this);

    //创建一个服务，call这个服务的时候，就发布目标的关节值
    grip_enable_srvs_ = nh_.advertiseService(
        "arm_kinematic/grip_enable", &ArmKinematic::GripEnableCallback, this);

    //创建一个服务，call这个服务，证明抓取已经运行完了
    conf_gen_unlock_srvs_ = nh_.advertiseService(
        "arm_kinematic/unlock", &ArmKinematic::UnlockCallback, this);

    //创建一个服务，call这个服务的时候，回到原来的机械臂位置
    go_default_srvs_ = nh_.advertiseService(
        "arm_kinematic/go_default", &ArmKinematic::GoDefaultCallback, this);

        
    grip_action_flag_ = true;
    grip_ongoing_flag_ = false;
    grip_stamp_ = ros::Time::now();
    arm_conf_.waist = 0.0;
  }

  ~ArmKinematic() = default;

 private:
  void InverseKine(const geometry_msgs::TransformStamped::ConstPtr &target) {
    ROS_INFO("KINE HAS RECEIVE INVERSE");
    double d1 = 0.291, d2 = 0.4, d3 = 0.41;  // d3 is approxd
    //d1应该是0.334，本来是0.35，d3应该是0.486，现在写的0.43
    // coord trans with origin at both joint1 and joint2
    double target_x = target->transform.translation.x + 0.1345;//这错了，因为baselink问题
    double target_y = target->transform.translation.y;
    // std::cout << "y: " << target_y <<std::endl;
    double target_z = target->transform.translation.z - d1;//到地面的距离，从关节2那
    double target_dis_xy = std::sqrt(target_x * target_x + target_y * target_y);
    double target_dis = std::sqrt(target_dis_xy * target_dis_xy +
                                  target_z * target_z);

    double theta1 = std::atan2(target_y, target_x);  // in rad
    assert(theta1 >= -(150.0/180.0)*M_PI && theta1 <= (150.0/180.0)*M_PI);//把<<改成了<就没问题了


    // if (target_x <= 0) {
    //   theta1 = (theta1 > 0)?(theta1 - M_PI):(theta1 + M_PI);//不需要了
    //   assert(theta1 >= -M_PI && theta1 < M_PI);//把<<改成了<就没问题了
    // }

    double cos_theta2 = 
      (d2 * d2 + target_dis * target_dis - d3 * d3) / (2 * d2 * target_dis);
    double pitch = std::atan2(target_z, target_dis_xy);
    double theta2 = M_PI / 2 - (std::acos(cos_theta2) + pitch);

    double cos_theta3 = 
      (d2 * d2 + d3 * d3 - target_dis * target_dis) / (2 * d2 * d3);
    double theta3 = std::acos(cos_theta3) - M_PI;

    Quaterniond rotation4;
    tf::quaternionMsgToEigen (target->transform.rotation, rotation4);
    Eigen::Matrix3d rotation_matrix4;
    rotation_matrix4 = rotation4.matrix();
    Vector3d rotation4_vector_ = rotation_matrix4.col(1);
    rotation4_vector_.normalize();

    Vector3d OX(1, 0, 0);
    Vector3d OY(0, 1, 0);
    double cos_theta4 = rotation4_vector_.dot(OY);
    Vector3d cross_ = rotation4_vector_.cross(OY);
    double sin_theta4 = cross_.norm();
    double theta4 = 0;

    if(cross_.dot(OX) < 0){
      theta4 = - atan2(sin_theta4, cos_theta4);
    }
    else if(cross_.dot(OX) > 0){
      theta4 = atan2(sin_theta4, cos_theta4);
    }
    else{
      ROS_ERROR("theta4 angle maybe wrong!");
      theta4 = 0;
    }

    if(theta4 < -M_PI/2){
      theta4 = theta4 + M_PI;
    }
    else if(theta4 > M_PI/2){
      theta4 = theta4 - M_PI;
    }
    else{}

    double theta4_up = (88.0/180.0)*M_PI;
    if(theta4 > theta4_up){
      theta4 = theta4_up;
    }
    else if (theta4 < -theta4_up){
      theta4 = -theta4_up;
    }
    else{}

    // assert(theta4 >= -(88/180)*M_PI && theta1 <= (88/180)*M_PI);

    arm_base::joint_angular arm_conf;
    arm_conf.waist = -theta1 * 180.0 / M_PI + 1;  // 1 deg is bias fix
    arm_conf.shoulder = -theta2 * 180.0 / M_PI;
    arm_conf.elbow = -theta3 * 180.0 / M_PI;
    arm_conf.wrist = theta4 * 180.0 / M_PI;
    arm_conf_.life_time = 20.0;
    std::cout << arm_conf.waist << " " << arm_conf.shoulder << " "
              << arm_conf.elbow << " " << arm_conf.wrist << std::endl;
      nav_conf_pub_.publish(arm_conf);//发布目标关节角度
      grip_stamp_ = ros::Time::now();
    // arm configuration publish 如果可以抓，并且不在抓取过程中，就发布
    //这个咋整啊
    // if (grip_action_flag_ && !grip_ongoing_flag_) {//第一个参数默认是false，如果得到服务，就会变成true
    //   nav_conf_pub_.publish(arm_conf);//发布目标关节角度
    //   grip_stamp_ = ros::Time::now();
    //   grip_action_flag_ = false;  // only once除非下次再call服务，否则不可能再次发布这个关节角度
    //   grip_ongoing_flag_ = true;
    // }

    if ((ros::Time::now() - grip_stamp_).toSec() > 85 &&
        grip_ongoing_flag_) {//如果超时25秒，并且还在抓取状态，就整个超时，其实没有必要了，这玩意肯定会反馈回来，除非那边程序挂了，这个可能就是提高鲁棒性
      grip_ongoing_flag_ = false;
      ROS_ERROR("Grip overtime!!!");
    }  // overtime wathcer
  }

  bool GripEnableCallback(std_srvs::EmptyRequest &request,
                          std_srvs::EmptyResponse &response) {
    grip_action_flag_ = true;//创建这个服务，就是为了吧这个action整成true
    ROS_INFO("Grip enable!");
    return true;
  }

  bool UnlockCallback(std_srvs::EmptyRequest &request,
                      std_srvs::EmptyResponse &response) {
    grip_ongoing_flag_ = false;
    ROS_INFO("Arm configuration gen unlock!");
    return true;
  }

  bool GoDefaultCallback(std_srvs::EmptyRequest &request,
                         std_srvs::EmptyResponse &response) {
    if (grip_ongoing_flag_) return false;
    arm_conf_.waist = 0.0;
    arm_conf_.shoulder = 50.0;
    arm_conf_.elbow = 150.0;
    arm_conf_.wrist = 0.0;
    arm_conf_.life_time = 10.0;  // overtime thres
    arm_conf_pub_.publish(arm_conf_);
    return true;
  }

  void SearchCallback(const std_msgs::UInt8::ConstPtr &cmd) {
    if ((ros::Time::now() - grip_stamp_).toSec() > 85 &&
        grip_ongoing_flag_) {//这是超时处理？超时之后就回来进行搜索，如果不在搜索状态也会搜索
      grip_ongoing_flag_ = false;
      ROS_ERROR("Grip overtime!!!");
    }
    else if (grip_ongoing_flag_) {
      ROS_WARN("Nav or Grip ongoing!! Don't accept search");  // reject scout cmd while grip ongoing
      return;
    }
    switch (cmd->data) {
      case 0:  // go default 在哪发布0呢
        arm_conf_.waist = 0.0;
        arm_conf_.shoulder = 50.0;
        arm_conf_.elbow = 150.0;
        arm_conf_.wrist = 0.0;
        arm_conf_.life_time = 10.0;
        arm_conf_pub_.publish(arm_conf_);
        break;
      case 1:  // clockwise search
        arm_conf_.waist += 15.0;  // accuracy is not important
        if (arm_conf_.waist > 50.0) arm_conf_.waist = 45.0; 
        arm_conf_.shoulder = 50.0;
        arm_conf_.elbow = 150.0;
        arm_conf_.wrist = 0.0;
        arm_conf_.life_time = 4.0;
        arm_conf_pub_.publish(arm_conf_);
        break;
      case 2:  // counter-clockwise search
        arm_conf_.waist -= 15.0;
        if (arm_conf_.waist < -50.0) arm_conf_.waist = -45.0;
        arm_conf_.shoulder = 50.0;
        arm_conf_.elbow = 150.0;
        arm_conf_.wrist = 0.0;
        arm_conf_.life_time = 4.0;
        arm_conf_pub_.publish(arm_conf_);
        break;
      default: break;
    }
  }

  ros::NodeHandle nh_;
  ros::Subscriber target_pose_sub_;
  ros::Subscriber search_cmd_sub_;
  ros::Publisher arm_conf_pub_;
  ros::Publisher nav_conf_pub_;
  ros::ServiceServer grip_enable_srvs_;
  ros::ServiceServer conf_gen_unlock_srvs_;
  ros::ServiceServer go_default_srvs_;
  arm_base::joint_angular arm_conf_;
  ros::Time grip_stamp_;
  bool grip_action_flag_;
  bool grip_ongoing_flag_;
};

int main (int argc, char **argv) {
  ros::init(argc, argv, "arm_kine_node");
  ArmKinematic arm_kinematic;
  ros::spin();
  return 0;
}